#include <iostream>
#include <fstream>
#include <dalelib/maths.h>
#include <dalelib/alife/robot.h>

namespace dalelib
{

Robot::Robot(dWorldID _World, dSpaceID _Space)
	: _world(_World)
	, _space(_Space)
{
	_jointgroup = dJointGroupCreate(0);
}

Robot::~Robot()
{
	clean();
}

void Robot::clean()
{
	for(PartList::iterator p=_parts.begin(); p!=_parts.end(); ++p) delete (*p);
	_parts.clear();
	for(JointList::iterator j=_joints.begin(); j!=_joints.end(); ++j) delete (*j);
	_joints.clear();
	_geompartmap.clear();
	dJointGroupEmpty(_jointgroup);
	dJointGroupDestroy(_jointgroup);
}

void Robot::reset()
{
	for(PartList::iterator p=_parts.begin(); p!=_parts.end(); ++p) (*p)->reset();
}

void Robot::update()
{
	for(JointList::iterator j=_joints.begin(); j!=_joints.end(); ++j) (*j)->update();
	for(PartList::iterator p=_parts.begin(); p!=_parts.end(); ++p) (*p)->update();
}

void Robot::draw()
{
	for(PartList::iterator p=_parts.begin(); p!=_parts.end(); ++p) (*p)->draw();
}

void Robot::drawJoints()
{
	glDisable(GL_LIGHTING);
	glColor4f(1,0,0,1);
	glBegin(GL_LINES);
	for(JointList::iterator j=_joints.begin(); j!=_joints.end(); ++j) (*j)->draw();
	glEnd();
	glColor4f(1,1,1,1);
}

v3 Robot::getAveragePosition()
{
	v3 pos(0,0,0);
	for(PartList::iterator p=_parts.begin(); p!=_parts.end(); ++p) pos += (*p)->getPosition();
	return pos/_parts.size();
}

void Robot::translate(const v3 _Pos)
{
	for(PartList::iterator p=_parts.begin(); p!=_parts.end(); ++p) (*p)->translate(_Pos);
}

void Robot::rotate(m3 _Rot)
{
/*
	for(PartList::iterator p=_parts.begin(); p!=_parts.end(); ++p)
	{
		(*p)->rotate(_Rot);
	}
*/
}

RobotJoint* Robot::addHingeJoint(const int _B0, const int _B1, const float _Strength, const v3 _Pos, const v3 _Dir)
{
	const dBodyID b0 = (_B0!=-1) ? _parts[_B0]->getBody() : 0;
	const dBodyID b1 = (_B1!=-1) ? _parts[_B1]->getBody() : 0;
	RobotJoint* joint = (RobotJoint*) new RobotHingeJoint( this, b0, b1, _Pos, _Dir, _Strength );
	_joints.push_back(joint);
	return joint;
}

RobotPart* Robot::addCapsule(const float _Radius, const v3 _P0, const v3 _P1, const float _Density)
{
	RobotPart* part = (RobotPart*) new RobotCapsule( (dxWorld*)_world, (dxSpace*)_space, _Radius, _P0, _P1, _Density );
	part->getID() = _parts.size();
	_parts.push_back(part);
	_geompartmap[part->getGeom()] = part;
	return (RobotPart*)part;
}

RobotPart* Robot::addBox(const float _Radius, const v3 _P0, const v3 _P1, const float _Density)
{
	RobotPart* part = (RobotPart*) new RobotBox( (dxWorld*)_world, (dxSpace*)_space, _Radius, _P0, _P1, _Density );
	part->getID() = _parts.size();
	_parts.push_back(part);
	_geompartmap[part->getGeom()] = part;
	return (RobotPart*)part;
}

void Robot::load(const string _FileName)
{
	ifstream fin( _FileName.c_str() );
	if( !fin ) { cout << "Error: couldn't load robot: " << _FileName << endl; return; }

	string str;
	do
	{
		fin >> str;
		if( !str.compare("SPHERE") )
		{
		}

		if( !str.compare("CAPSULE") )
		{
			float size; // radius
			float d = 500.f;
			v3 pos1, pos2;
			fin >> size >> pos1[0] >> pos1[1] >> pos1[2] >> pos2[0] >> pos2[1] >> pos2[2];
			addCapsule( size, pos1, pos2, d );
		}

		if( !str.compare("CYLINDER") )
		{
		}

		if( !str.compare("BOX") )
		{
			v3 size; // radius
			float d = 500.f;
			v3 pos1, pos2;
			fin >> size[0] >> pos1[0] >> pos1[1] >> pos1[2] >> pos2[0] >> pos2[1] >> pos2[2];
//			addPart( RobotPart::BOX, size, pos1, pos2, d );
		}

		if( !str.compare("JOINT") )
		{
			int p1, p2;
			float str; // radius
			v3 axis, up;
			fin >> p1 >> p2 >> str >> axis[0] >> axis[1] >> axis[2] >> up[0] >> up[1] >> up[2];
			addHingeJoint( p1, p2, str, axis, up );
		}

		str = "";
	} while( !fin.eof() );
	fin.close();
}

} // namespace dalelib
